﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/GearJoint.h
// Revision   : r29
// Change-Date: 2009-11-28
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/GearJoint.cpp
// Revision   : r29
// Change-Date: 2009-11-28
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Gear joint definition. This definition requires two existing 
    /// revolute or prismatic joints (any combination will work). 
    /// The provided joints must attach a dynamic body to a static body.
    /// </summary>
    public class GearJointDef : JointDef
    {
        /// <summary>
        /// The first revolute/prismatic joint attached to the gear joint.
        /// </summary>
        public Joint joint1;

        /// <summary>
        /// The second revolute/prismatic joint attached to the gear joint.
        /// </summary>
        public Joint joint2;

        /// <summary>
        /// The gear ratio. see GearJoint for explanation.
        /// </summary>
        public float ratio;

        public GearJointDef()
        {
            type = JointType.GEAR;
            joint1 = null;
            joint2 = null;
            ratio = 1.0f;
        }
    }

    /// <summary>
    /// A gear joint is used to connect two joints together. Either joint 
    /// can be a revolute or prismatic joint. You specify a gear ratio 
    /// to bind the motions together: 
    /// coordinate1 + ratio * coordinate2 = constant 
    /// The ratio can be negative or positive. If one joint is a revolute joint 
    /// and the other joint is a prismatic joint, then the ratio will have units 
    /// of length or units of 1/length. 
    /// @warning The revolute and prismatic joints must be attached to 
    /// fixed bodies (which must be body1 on those joints). 
    /// </summary>
    public class GearJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchor1);
        }

	    public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchor2);
        }

	    public override Vector2 GetReactionForce(float inv_dt)
        {
            // TODO_ERIN not tested
            Vector2 P = m_impulse * m_J.linear2;
            return inv_dt * P;
        }

	    public override float GetReactionTorque(float inv_dt)
        {
            // TODO_ERIN not tested
            Vector2 r = Math.Mul(m_body2.Transform.R, m_localAnchor2 - m_body2.LocalCenter);
            Vector2 P = m_impulse * m_J.linear2;
            float L = m_impulse * m_J.angular2 - Vector2Extensions.Cross(r, P);
            return inv_dt * L;
        }

        public void SetRatio(float ratio)
        {
            System.Diagnostics.Debug.Assert(Math.IsValid(ratio));
            m_ratio = ratio;
        }

	    /// Get the gear ratio.
	    public float GetRatio()
        {
            return m_ratio;
        }


        public GearJoint(GearJointDef def)
            : base(def)
        {
            JointType type1 = def.joint1.GetType();
            JointType type2 = def.joint2.GetType();

            System.Diagnostics.Debug.Assert(type1 == JointType.REVOLUTE || type1 == JointType.PRISMATIC);
            System.Diagnostics.Debug.Assert(type2 == JointType.REVOLUTE || type2 == JointType.PRISMATIC);
            System.Diagnostics.Debug.Assert(def.joint1.GetBodyA().Type == BodyType.STATIC);
            System.Diagnostics.Debug.Assert(def.joint2.GetBodyA().Type == BodyType.STATIC);

            m_revolute1 = null;
            m_prismatic1 = null;
            m_revolute2 = null;
            m_prismatic2 = null;

            float coordinate1, coordinate2;

            m_ground1 = def.joint1.GetBodyA();
            m_body1 = def.joint1.GetBodyB();
            if (type1 == JointType.REVOLUTE)
            {
                m_revolute1 = (RevoluteJoint)def.joint1;
                m_groundAnchor1 = m_revolute1.m_localAnchor1;
                m_localAnchor1 = m_revolute1.m_localAnchor2;
                coordinate1 = m_revolute1.GetJointAngle();
            }
            else
            {
                m_prismatic1 = (PrismaticJoint)def.joint1;
                m_groundAnchor1 = m_prismatic1.m_localAnchor1;
                m_localAnchor1 = m_prismatic1.m_localAnchor2;
                coordinate1 = m_prismatic1.GetJointTranslation();
            }

            m_ground2 = def.joint2.GetBodyA();
            m_body2 = def.joint2.GetBodyB();
            if (type2 == JointType.REVOLUTE)
            {
                m_revolute2 = (RevoluteJoint)def.joint2;
                m_groundAnchor2 = m_revolute2.m_localAnchor1;
                m_localAnchor2 = m_revolute2.m_localAnchor2;
                coordinate2 = m_revolute2.GetJointAngle();
            }
            else
            {
                m_prismatic2 = (PrismaticJoint)def.joint2;
                m_groundAnchor2 = m_prismatic2.m_localAnchor1;
                m_localAnchor2 = m_prismatic2.m_localAnchor2;
                coordinate2 = m_prismatic2.GetJointTranslation();
            }

            m_ratio = def.ratio;

            m_constant = coordinate1 + m_ratio * coordinate2;

            m_impulse = 0.0f;
        }

	    internal override void InitVelocityConstraints(TimeStep step)
        {
            Body g1 = m_ground1;
            Body g2 = m_ground2;
            Body b1 = m_body1;
            Body b2 = m_body2;

            float K = 0.0f;
            m_J.SetZero();

            if (m_revolute1 != null)
            {
                m_J.angular1 = -1.0f;
                K += b1.m_invI;
            }
            else
            {
                Vector2 ug = Math.Mul(g1.Transform.R, m_prismatic1.m_localXAxis1);
                Vector2 r = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
                float crug = Vector2Extensions.Cross(r, ug);
                m_J.linear1 = -ug;
                m_J.angular1 = -crug;
                K += b1.m_invMass + b1.m_invI * crug * crug;
            }

            if (m_revolute2 != null)
            {
                m_J.angular2 = -m_ratio;
                K += m_ratio * m_ratio * b2.m_invI;
            }
            else
            {
                Vector2 ug = Math.Mul(g2.Transform.R, m_prismatic2.m_localXAxis1);
                Vector2 r = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);
                float crug = Vector2Extensions.Cross(r, ug);
                m_J.linear2 = -m_ratio * ug;
                m_J.angular2 = -m_ratio * crug;
                K += m_ratio * m_ratio * (b2.m_invMass + b2.m_invI * crug * crug);
            }

            // Compute effective mass.
            m_mass = K > 0.0f ? 1.0f / K : 0.0f;

            if (step.warmStarting)
            {
                // Warm starting.
                b1.m_linearVelocity += b1.m_invMass * m_impulse * m_J.linear1;
                b1.m_angularVelocity += b1.m_invI * m_impulse * m_J.angular1;
                b2.m_linearVelocity += b2.m_invMass * m_impulse * m_J.linear2;
                b2.m_angularVelocity += b2.m_invI * m_impulse * m_J.angular2;
            }
            else
            {
                m_impulse = 0.0f;
            }
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = m_body1;
            Body b2 = m_body2;

            float Cdot = m_J.Compute(b1.m_linearVelocity, b1.m_angularVelocity, b2.m_linearVelocity, b2.m_angularVelocity);

            float impulse = m_mass * (-Cdot);
            m_impulse += impulse;

            b1.m_linearVelocity += b1.m_invMass * impulse * m_J.linear1;
            b1.m_angularVelocity += b1.m_invI * impulse * m_J.angular1;
            b2.m_linearVelocity += b2.m_invMass * impulse * m_J.linear2;
            b2.m_angularVelocity += b2.m_invI * impulse * m_J.angular2;
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            //B2_NOT_USED(baumgarte);

            float linearError = 0.0f;

            Body b1 = m_body1;
            Body b2 = m_body2;

            float coordinate1, coordinate2;
            if (m_revolute1 != null)
            {
                coordinate1 = m_revolute1.GetJointAngle();
            }
            else
            {
                coordinate1 = m_prismatic1.GetJointTranslation();
            }

            if (m_revolute2 != null)
            {
                coordinate2 = m_revolute2.GetJointAngle();
            }
            else
            {
                coordinate2 = m_prismatic2.GetJointTranslation();
            }

            float C = m_constant - (coordinate1 + m_ratio * coordinate2);

            float impulse = m_mass * (-C);

            b1.m_sweep.c += b1.m_invMass * impulse * m_J.linear1;
            b1.m_sweep.a += b1.m_invI * impulse * m_J.angular1;
            b2.m_sweep.c += b2.m_invMass * impulse * m_J.linear2;
            b2.m_sweep.a += b2.m_invI * impulse * m_J.angular2;

            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            // TODO_ERIN not implemented
            return linearError < PhysicSettings.LinearSlop;
        }

	    internal Body m_ground1;
	    internal Body m_ground2;

	    // One of these is null.
	    internal RevoluteJoint m_revolute1;
	    internal PrismaticJoint m_prismatic1;

	    // One of these is null.
	    internal RevoluteJoint m_revolute2;
	    internal PrismaticJoint m_prismatic2;

	    internal Vector2 m_groundAnchor1;
	    internal Vector2 m_groundAnchor2;

	    internal Vector2 m_localAnchor1;
	    internal Vector2 m_localAnchor2;

	    internal Jacobian m_J;

	    internal float m_constant;
	    internal float m_ratio;

	    // Effective mass
	    internal float m_mass;

	    // Impulse for accumulation/warm starting.
	    internal float m_impulse;
    }
}
